/*
**  jstest-gtk - A graphical joystick tester
**  Copyright (C) 2009 Ingo Ruhnke <grumbel@gmail.com>
**
**  This program is free software: you can redistribute it and/or modify
**  it under the terms of the GNU General Public License as published by
**  the Free Software Foundation, either version 3 of the License, or
**  (at your option) any later version.
**
**  This program is distributed in the hope that it will be useful,
**  but WITHOUT ANY WARRANTY; without even the implied warranty of
**  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
**  GNU General Public License for more details.
**
**  You should have received a copy of the GNU General Public License
**  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <assert.h>
#include <math.h>
#include <algorithm>
#include <iostream>
#include <stdexcept>
#include <stdint.h>
#include <string.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sstream>
#include <unistd.h>
#include <linux/joystick.h>
#include "evdev_helper.hpp"
#include "joystick.h"

// 构造函数 Joystick_pub，用于初始化一个 Joystick_pub 对象
// 参数 filename_ 是 joystick 设备文件的路径
Joystick_pub::Joystick_pub(const std::string &filename, const std::string &node_name)
    : filename(filename), Node(node_name) // 初始化成员变量 filename
{
    heartbeat = 0;
    // 尝试以只读模式打开 joystick 设备文件
    if ((fd = open(filename.c_str(), O_RDONLY | O_NONBLOCK)) < 0)
    {
        // 如果打开失败，抛出 runtime_error 异常，并附带错误信息
        std::ostringstream str;
        str << filename << ": " << strerror(errno);
        throw std::runtime_error(str.str());
    }
    else
    {
        // 获取设备上的轴数和按钮数
        uint8_t num_axis = 0;                  // 存储轴数量
        uint8_t num_button = 0;                // 存储按钮数量
        ioctl(fd, JSIOCGAXES, &num_axis);      // 使用 ioctl 获取轴数量
        ioctl(fd, JSIOCGBUTTONS, &num_button); // 使用 ioctl 获取按钮数量
        axis_count = num_axis;                 // 设置 axis_count 成员变量
        button_count = num_button;             // 设置 button_count 成员变量

        // 获取设备名称
        char name_c_str[1024]; // 临时存储设备名称的字符数组
        if (ioctl(fd, JSIOCGNAME(sizeof(name_c_str)), name_c_str) < 0)
        {
            // 如果获取名称失败，抛出 runtime_error 异常，并附带错误信息
            std::ostringstream str;
            str << filename << ": " << strerror(errno);
            throw std::runtime_error(str.str());
        }
        else
        {
            // 获取成功，保存原始名称
            orig_name = name_c_str;
            name = orig_name; // 保存设备名称
        }

        // 初始化轴状态向量，大小等于轴的数量
        axis_state.resize(axis_count);
        button_state.reserve(button_count);
    }

    // 获取并保存初始校准数据
    orig_calibration_data = get_calibration();

    // 输出设备信息
    std::cout << "Joystick_pub device info:" << std::endl;
    std::cout << "Name: " << name << std::endl;
    std::cout << "Number of Axes: " << axis_count << std::endl;
    std::cout << "Number of Buttons: " << button_count << std::endl;

    g29_publisher = this->create_publisher<g29_msg::msg::G29Msg>("g29_feedback", 10);
    g29_pub_timer = this->create_wall_timer(std::chrono::milliseconds(10), std::bind(&Joystick_pub::on_in_publish, this));
}

Joystick_pub::~Joystick_pub()
{
    // connection.disconnect();
    close(fd);
}

bool Joystick_pub::on_in_publish()
{
    update();  // 读取并更新摇杆状态
    publish(); // 发布摇杆按钮
    return true;
}

/**
 * 更新摇杆状态
 *
 * 从摇杆设备文件中读取事件，并根据事件更新摇杆的状态
 * 如果读取事件失败或发生错误，将抛出异常
 */
void Joystick_pub::update()
{
    // 定义一个 js_event 结构体来存储读取的事件
    struct js_event event;
    // 从摇杆设备文件中读取事件
    ssize_t len = read(fd, &event, sizeof(event));
    heartbeat++;
    if (heartbeat > 100)
    {
        heartbeat = 0;
    }
    // 如果读取长度为负值，则读取失败，抛出异常
    if (len < 0)
    {
        return;
        // 构建错误信息
        // std::ostringstream str;
        // str << filename << ": " << strerror(errno);
        // // 抛出运行时错误异常
        // throw std::runtime_error(str.str());
    }
    // 如果读取长度与事件大小一致，则读取成功
    else if (len == sizeof(event))
    {
        // 如果事件类型包含轴事件
        if (event.type & JS_EVENT_AXIS)
        {
            axis_update(event.number, event.value);
        }
        // 如果事件类型包含按钮事件
        else if (event.type & JS_EVENT_BUTTON)
        {
            // 调用按钮移动的处理函数
            button_update(event.number, event.value);
        }
    }
    // 如果读取长度既不为负值也不为事件大小，则发生未知错误，抛出异常
    else
    {
        throw std::runtime_error("Joystick_pub::update(): unknown read error");
    }
}

void Joystick_pub::publish()
{
    auto message = g29_msg::msg::G29Msg();

    message.heartbeat = heartbeat;
    message.axis0 = axis_state[0];
    message.axis1 = axis_state[1];
    message.axis2 = axis_state[2];
    message.axis3 = axis_state[3];
    message.axis4 = axis_state[4];
    message.axis5 = axis_state[5];

    message.button0 = button_state[0];
    message.button1 = button_state[1];
    message.button2 = button_state[2];
    message.button3 = button_state[3];
    message.button4 = button_state[4];
    message.button5 = button_state[5];
    message.button6 = button_state[6];
    message.button7 = button_state[7];
    message.button8 = button_state[8];
    message.button9 = button_state[9];
    message.button10 = button_state[10];
    message.button11 = button_state[11];
    message.button12 = button_state[12];
    message.button13 = button_state[13];
    message.button14 = button_state[14];
    message.button15 = button_state[15];
    message.button16 = button_state[16];
    message.button17 = button_state[17];
    message.button18 = button_state[18];
    message.button19 = button_state[19];
    message.button20 = button_state[20];
    message.button21 = button_state[21];
    message.button22 = button_state[22];
    message.button23 = button_state[23];
    message.button24 = button_state[24];

    g29_publisher->publish(message);
}

// 轴移动的处理函数
void Joystick_pub::axis_update(int axis, int value)
{
    // 更新轴的状态
    axis_state[axis] = value;
    // 调用轴移动的处理函数
    // std::cout << "Axis " << axis << " moved to value: " << axis_state[axis] << std::endl;
}

// 按钮移动的处理函数
void Joystick_pub::button_update(int button, int value)
{
    // 处理按钮事件
    button_state[button] = value;
    // std::cout << "Button " << button << " moved to value: " << button_state[button] << std::endl;
}

Joystick_pub::CalibrationData corr2cal(const struct js_corr &corr_)
{
    struct js_corr corr = corr_;

    Joystick_pub::CalibrationData data;

    if (corr.type)
    {
        data.calibrate = true;
        data.invert = (corr.coef[2] < 0 && corr.coef[3] < 0);
        data.center_min = corr.coef[0];
        data.center_max = corr.coef[1];

        if (data.invert)
        {
            corr.coef[2] = -corr.coef[2];
            corr.coef[3] = -corr.coef[3];
        }

        // Need to use double and rint(), since calculation doesn't end
        // up on clean integer positions (i.e. 0.9999 can happen)
        data.range_min = rint(data.center_min - ((32767.0 * 16384) / corr.coef[2]));
        data.range_max = rint((32767.0 * 16384) / corr.coef[3] + data.center_max);
    }
    else
    {
        data.calibrate = false;
        data.invert = false;
        data.center_min = 0;
        data.center_max = 0;
        data.range_min = 0;
        data.range_max = 0;
    }

    return data;
}

std::vector<Joystick_pub::CalibrationData> Joystick_pub::get_calibration()
{
    std::vector<struct js_corr> corr(get_axis_count());

    if (ioctl(fd, JSIOCGCORR, &*corr.begin()) < 0)
    {
        std::ostringstream str;
        str << filename << ": " << strerror(errno);
        throw std::runtime_error(str.str());
    }
    else
    {
        std::vector<CalibrationData> data;
        std::transform(corr.begin(), corr.end(), std::back_inserter(data), corr2cal);
        return data;
    }
}

struct js_corr cal2corr(const Joystick_pub::CalibrationData &data)
{
    struct js_corr corr;

    if (data.calibrate &&
        (data.center_min - data.range_min) != 0 &&
        (data.range_max - data.center_max) != 0)
    {
        corr.type = 1;
        corr.prec = 0;
        corr.coef[0] = data.center_min;
        corr.coef[1] = data.center_max;

        corr.coef[2] = (32767 * 16384) / (data.center_min - data.range_min);
        corr.coef[3] = (32767 * 16384) / (data.range_max - data.center_max);

        if (data.invert)
        {
            corr.coef[2] = -corr.coef[2];
            corr.coef[3] = -corr.coef[3];
        }
    }
    else
    {
        corr.type = 0;
        corr.prec = 0;
        memset(corr.coef, 0, sizeof(corr.coef));
    }

    return corr;
}

void Joystick_pub::set_calibration(const std::vector<CalibrationData> &data)
{
    std::vector<struct js_corr> corr;

    std::transform(data.begin(), data.end(), std::back_inserter(corr), cal2corr);

    if (ioctl(fd, JSIOCSCORR, &*corr.begin()) < 0)
    {
        std::ostringstream str;
        str << filename << ": " << strerror(errno);
        throw std::runtime_error(str.str());
    }
}

void Joystick_pub::clear_calibration()
{
    std::vector<CalibrationData> data;

    for (int i = 0; i < get_axis_count(); ++i)
    {
        CalibrationData cal;

        cal.calibrate = false;
        cal.invert = false;
        cal.center_min = 0;
        cal.center_max = 0;
        cal.range_min = 0;
        cal.range_max = 0;

        data.push_back(cal);
    }

    set_calibration(data);
}

void Joystick_pub::reset_calibration()
{
    set_calibration(orig_calibration_data);
}

std::vector<int> Joystick_pub::get_button_mapping()
{
    uint16_t btnmap[KEY_MAX - BTN_MISC + 1];
    if (ioctl(fd, JSIOCGBTNMAP, btnmap) < 0)
    {
        std::ostringstream str;
        str << filename << ": " << strerror(errno);
        throw std::runtime_error(str.str());
    }
    else
    {
        std::vector<int> mapping;
        std::copy(btnmap, btnmap + button_count, std::back_inserter(mapping));
        return mapping;
    }
}

std::vector<int> Joystick_pub::get_axis_mapping()
{
    uint8_t axismap[ABS_MAX + 1];
    if (ioctl(fd, JSIOCGAXMAP, axismap) < 0)
    {
        std::ostringstream str;
        str << filename << ": " << strerror(errno);
        throw std::runtime_error(str.str());
    }
    else
    {
        std::vector<int> mapping;
        std::copy(axismap, axismap + axis_count, std::back_inserter(mapping));
        return mapping;
    }
}

void Joystick_pub::set_button_mapping(const std::vector<int> &mapping)
{
    assert((int)mapping.size() == button_count);

    uint16_t btnmap[KEY_MAX - BTN_MISC + 1];
    memset(btnmap, 0, sizeof(btnmap));
    std::copy(mapping.begin(), mapping.end(), btnmap);

    if (0)
        for (int i = 0; i < button_count; ++i)
        {
            std::cout << i << " -> " << btnmap[i] << std::endl;
        }

    if (ioctl(fd, JSIOCSBTNMAP, btnmap) < 0)
    {
        std::ostringstream str;
        str << filename << ": " << strerror(errno);
        throw std::runtime_error(str.str());
    }
}

int Joystick_pub::get_axis_state(int id)
{
    if (id >= 0 && id < (int)axis_state.size())
    {
        return axis_state[id];
    }
    else
    {
        return 0;
    }
}

int Joystick_pub::get_button_state(int id)
{
    if (id >= 0 && id < get_button_count())
    {
        return button_state[id];
    }
    else
    {
        return 0;
    }
}

void Joystick_pub::set_axis_mapping(const std::vector<int> &mapping)
{
    assert((int)mapping.size() == axis_count);

    uint8_t axismap[ABS_MAX + 1];

    std::copy(mapping.begin(), mapping.end(), axismap);

    if (ioctl(fd, JSIOCSAXMAP, axismap) < 0)
    {
        std::ostringstream str;
        str << filename << ": " << strerror(errno);
        throw std::runtime_error(str.str());
    }
}

void Joystick_pub::correct_calibration(const std::vector<int> &mapping_old, const std::vector<int> &mapping_new)
{
    int axes[ABS_MAX + 1]; // axes[name] -> old_idx
    for (std::vector<int>::const_iterator i = mapping_old.begin(); i != mapping_old.end(); ++i)
    {
        axes[*i] = i - mapping_old.begin();
    }

    std::vector<CalibrationData> callib_old = get_calibration();
    std::vector<CalibrationData> callib_new;
    for (std::vector<int>::const_iterator i = mapping_new.begin(); i != mapping_new.end(); ++i)
    {
        callib_new.push_back(callib_old[axes[*i]]);
    }

    set_calibration(callib_new);
}

std::string Joystick_pub::get_evdev() const
{
    // See /usr/share/doc/linux-doc-2.6.28/devices.txt.gz
    for (int i = 0; i < 32; ++i)
    {
        std::ostringstream out;
        out << "/dev/input/event" << i;

        int evdev_fd;
        if ((evdev_fd = open(out.str().c_str(), O_RDONLY)) < 0)
        {
            // ignore
        }
        else
        {
            char evdev_name[256];
            if (ioctl(evdev_fd, EVIOCGNAME(sizeof(evdev_name)), evdev_name) < 0)
            {
                std::cout << out.str() << ": " << strerror(errno) << std::endl;
            }
            else
            {
                if (orig_name == evdev_name)
                {
                    // Found a device that matches, so return it
                    close(evdev_fd);
                    return out.str();
                }
            }

            close(evdev_fd);
        }
    }

    throw std::runtime_error("couldn't find evdev for " + filename);
}

/* EOF */
